Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Try fix robot start state with STOMP #26

Merged
merged 1 commit into from
Oct 20, 2021
Merged

Try fix robot start state with STOMP #26

merged 1 commit into from
Oct 20, 2021

Conversation

s-trinh
Copy link
Contributor

@s-trinh s-trinh commented Jun 25, 2020

I have no experience with MoveIt code.

These changes seems to solve the following issue for me:

  • I am using OMPL + STOMP similarly to the OMPL + CHOMP tutorial
  • I am using the MoveGroup plan()
  • without these changes, each new plan uses the default robot start state instead of the current robot state

I am not sure if STOMP is actually taking into account attached object for the optimization.

I don't know if all these changes are required and if this is the correct approach (should planning_scene_monitor be used instead?).
Maybe the current code is fine and it is intended to always provide full robot state in the request (no diff)? Maybe my changes are breaking this behaviour?

I think this issue is related.

@jrgnicho
Copy link
Member

How were you able to test if the desired behavior was produced?

@captain-yoshi captain-yoshi mentioned this pull request Jul 23, 2020
Open
@captain-yoshi
Copy link
Contributor

Without these changes, each new plan uses the default robot start state instead of the current robot state

I was having the same issue using MoveitTaskConstructor package. I can confirm that this pull request solves this issue.

How were you able to test if the desired behavior was produced?

I have attached an OMPL and a STOMP gif to demonstrate the issue (See top comment)

v4hn
v4hn approved these changes Jul 23, 2020
View reviewed changes
Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@cpt-yoshi you found a rather old under defined problem with the MoveIt planner interface and ironically @jrgnicho was the one originally pointing it out. After a lot of discussion multiple MoveIt maintainers agreed that it's best to use the robot state of the planning_scene provided with the request as the current state, and interpret the message in request.start_state as a diff on top. Ideally people would just forget about this start_state field on the side of the planner altogether.

As this is exactly the approach you implemented here, I approve.

@captain-yoshi
Copy link
Contributor

As this is exactly the approach you implemented here, I approve.

I just happened to come across this pull request. Credit goes to @s-trinh!

@akjay
Copy link
Contributor

akjay commented Aug 4, 2020

It seems still has some problem with the industrial_moveit_test_moveit_config pkg demo when use this fix to plan.

@v4hn
Copy link
Contributor

v4hn commented Aug 4, 2020 via email

@akjay
Copy link
Contributor

akjay commented Aug 4, 2020

It seems still has some problem with the industrial_moveit_test_moveit_config pkg demo when use this fix to plan.

This PR fixed the start state problem, but there still has a problem with bool Stomp::solve(const Eigen::MatrixXd& initial_parameters,Eigen::MatrixXd& parameters_optimized) function I think.Which a right initial_parameters will get a wrong parameters_optimized result.

@v4hn
Copy link
Contributor

v4hn commented Aug 4, 2020 via email

@akjay
Copy link
Contributor

akjay commented Aug 4, 2020

It seems still has some problem
Could you elaborate and show logs?

This PR fixed the start state problem, but there still has a problem withbool Stomp::solve(const Eigen::MatrixXd& initial_parameters,Eigen::MatrixXd& parameters_optimized)function I think.Which a right initial_parameters will get a wrong parameters_optimized result.
Here is a example.A plan problem with the demo industrial_moveit_test_moveit_config,while I set the start_state to start_tool_pose and goal to end_tool_pose.Firstly I always get wrong start_state by all zero, after I fixed this problem by this PR,I got another one,I am not sure this is an independent issue or not,but I tried many times and seemed locate the issue in function bool Stomp::solve(const Eigen::MatrixXd& initial_parameters,Eigen::MatrixXd& parameters_optimized).
The initial_parameters and parameters_optimized shows below, and I use the default config in the pkg.
[ WARN] [1596534836.803726586]: initial_parameters [ INFO] [1596534836.804106249]: 1.3483 1.32851 1.30837 1.28185 1.24931 1.21111 1.16762 1.11921 1.06623 1.00906 0.948059 0.883589 0.816018 0.74571 0.67303 0.598343 0.522014 0.444408 0.36589 0.286824 0.207576 0.12851 0.0499921 -0.0276139 -0.103943 -0.17863 -0.25131 -0.321618 -0.389189 -0.453659 -0.514662 -0.571834 -0.624809 -0.673224 -0.716712 -0.754909 -0.787451 -0.813971 -0.834107 -0.8539 1.1972 1.20013 1.20312 1.20705 1.21188 1.21754 1.22399 1.23117 1.23902 1.2475 1.25654 1.2661 1.27612 1.28654 1.29732 1.30839 1.31971 1.33121 1.34285 1.35458 1.36632 1.37805 1.38969 1.40119 1.41251 1.42358 1.43436 1.44478 1.4548 1.46436 1.4734 1.48188 1.48973 1.49691 1.50336 1.50902 1.51385 1.51778 1.52077 1.5237 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 -0.3137 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 0.676 -1.0295 -1.04986 -1.07057 -1.09784 -1.13131 -1.1706 -1.21533 -1.26512 -1.31961 -1.37841 -1.44115 -1.50746 -1.57696 -1.64927 -1.72403 -1.80084 -1.87935 -1.95917 -2.03993 -2.12125 -2.20275 -2.28407 -2.36483 -2.44465 -2.52316 -2.59997 -2.67473 -2.74704 -2.81654 -2.88285 -2.94559 -3.00439 -3.05888 -3.10867 -3.1534 -3.19269 -3.22616 -3.25343 -3.27414 -3.2945 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.2011 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 -1.51 [ INFO] [1596534836.924487088]: STOMP found a valid solution with cost 0.000000 after 3 iterations [ WARN] [1596534836.924769009]: parameters_optimized [ INFO] [1596534836.925156332]: 1.00002 1.11766 1.1955 1.23942 1.25476 1.24631 1.21835 1.17468 1.11864 1.05314 0.980694 0.903428 0.823129 0.741261 0.658997 0.577241 0.496659 0.417707 0.340653 0.265613 0.192569 0.121402 0.0519192 -0.0161221 -0.0829842 -0.148923 -0.214162 -0.27886 -0.343091 -0.406811 -0.469835 -0.531805 -0.59217 -0.650148 -0.704711 -0.754548 -0.798042 -0.833244 -0.857841 -0.869135 5.20494e-05 0.509067 0.877337 1.12957 1.2879 1.37197 1.39912 1.38444 1.34097 1.27979 1.21014 1.13957 1.07405 1.0181 0.974933 0.946562 0.933934 0.93706 0.955141 0.986692 1.02967 1.08162 1.13975 1.20112 1.26275 1.3217 1.37528 1.42111 1.45727 1.48244 1.496 1.49819 1.4902 1.47433 1.4541 1.43439 1.42153 1.42348 1.44994 1.51243 -1.36384e-05 -0.149305 -0.261962 -0.344658 -0.403314 -0.443131 -0.468635 -0.483713 -0.491653 -0.495189 -0.496532 -0.497418 -0.499142 -0.502599 -0.508328 -0.516543 -0.527183 -0.539943 -0.554319 -0.569645 -0.585135 -0.59992 -0.61309 -0.623732 -0.630972 -0.634011 -0.63217 -0.624924 -0.611945 -0.593142 -0.568699 -0.539115 -0.505245 -0.46834 -0.430083 -0.392634 -0.358664 -0.331401 -0.314664 -0.312907 2.93898e-05 0.30532 0.532111 0.694013 0.803193 0.870448 0.905273 0.915936 0.90955 0.892141 0.868723 0.843368 0.819278 0.798857 0.783781 0.775074 0.773172 0.778004 0.789054 0.805442 0.825987 0.849285 0.873778 0.897825 0.919775 0.938039 0.951158 0.957881 0.957231 0.948578 0.931713 0.906916 0.875031 0.837536 0.796613 0.755224 0.717177 0.687203 0.671025 0.675428 0.999912 0.26998 -0.277659 -0.675584 -0.952881 -1.13532 -1.24551 -1.30311 -1.32497 -1.32533 -1.31597 -1.3064 -1.30406 -1.31442 -1.34125 -1.38671 -1.45159 -1.53543 -1.63675 -1.75318 -1.88165 -2.01856 -2.15999 -2.3018 -2.43988 -2.57029 -2.68944 -2.79426 -2.88236 -2.95227 -3.00352 -3.03688 -3.05453 -3.06021 -3.05941 -3.05953 -3.07007 -3.10283 -3.17201 -3.29446 -5.2219e-05 -0.458803 -0.802331 -1.04997 -1.21903 -1.32494 -1.38128 -1.39994 -1.39121 -1.36384 -1.32519 -1.28128 -1.23696 -1.19593 -1.16089 -1.13362 -1.11509 -1.10556 -1.10466 -1.11153 -1.12486 -1.14305 -1.16428 -1.1866 -1.20805 -1.22676 -1.24104 -1.24946 -1.25099 -1.24509 -1.23179 -1.21179 -1.18658 -1.15854 -1.131 -1.10841 -1.09635 -1.10173 -1.13279 -1.19928 -6.56487e-05 -0.578207 -1.00904 -1.31745 -1.52572 -1.65368 -1.71878 -1.7363 -1.71942 -1.67937 -1.62556 -1.56569 -1.50592 -1.45095 -1.40421 -1.36792 -1.34326 -1.33052 -1.32916 -1.33801 -1.35537 -1.37913 -1.40692 -1.43622 -1.46452 -1.48939 -1.50868 -1.52061 -1.52391 -1.51792 -1.50279 -1.47953 -1.45018 -1.41795 -1.38731 -1.36417 -1.35596 -1.37179 -1.42257 -1.52113 [ INFO] [1596534836.927833756]: STOMP found a valid path after 0.132133 seconds [ INFO] [1596534836.927894042]: After adapter call fn [ INFO] [1596534836.927923092]: Motion planner reported a solution path with 40 states [ INFO] [1596534836.929324081]: Execution request received [ERROR] [1596534836.991672884]: Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'joint_1': expected: 5.20494e-05, current: 1.1972 [ INFO] [1596534836.991729658]: Execution completed: ABORTED

@akjay
Copy link
Contributor

akjay commented Aug 4, 2020

The log texts is a few confused.I do not know how to upload a picture.

@akjay
Copy link
Contributor

akjay commented Aug 5, 2020

there still has a problem with bool Stomp::solve(const Eigen::MatrixXd& initial_parameters,Eigen::MatrixXd& parameters_optimized) function I think.Which a right initial_parameters will get a wrong parameters_optimized result.
At first glance this is an independent issue, isn't it?

I have opened a new issue to talk about it.Here

@v4hn
Copy link
Contributor

v4hn commented Mar 22, 2021

@jrgnicho @gavanderhoorn (anyone else?)

I this is a rather small issue and the fix is clear.
Could you please merge this to have Stomp supported in MTC?

@v4hn v4hn mentioned this pull request Aug 12, 2021
Closed
@jrgnicho jrgnicho merged commit c9cf671 into ros-industrial:melodic-devel Oct 20, 2021
This was referenced Oct 20, 2021
Closed
Open
@rickstaa rickstaa mentioned this pull request Oct 21, 2021
Closed
26 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@v4hn v4hn v4hn approved these changes

Assignees
No one assigned
Labels
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
5 participants
@s-trinh @jrgnicho @captain-yoshi @akjay @v4hn